#include "cartographer_ros/trajectory_publisher.h"

namespace cartographer_ros
{
    TrajectoryPublisher::TrajectoryPublisher(rclcpp::Node::SharedPtr &node, const cartographer_ros::NodeOptions &node_options)
        : node_(node),
          node_options_(node_options)
    {
        tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node);
        if (node_options_.publish_tracked_pose)
        {
            tracked_pose_publisher_ = node_->create_publisher<::geometry_msgs::msg::PoseStamped>(kTrackedPoseTopic, 10);
        }
        scan_matched_publisher_ = node_->create_publisher<sensor_msgs::msg::PointCloud2>(kScanMatchedPointCloudTopic, 10);
    }

    void TrajectoryPublisher::Publish(const MapBuilderBridge::LocalTrajectoryData &trajectory_data,
                                      mapping::PoseExtrapolator &extrapolator, int traj_id)
    {
        const auto &map_frame = node_options_.map_frame;
        auto &local_data = trajectory_data.local_slam_data;
        auto &traj_options = trajectory_data.trajectory_options;
        bool use_pose_extra = node_options_.use_pose_extrapolator;
        // We only publish a point cloud if it has changed. It is not needed at high
        // frequency, and republishing it would be computationally wasteful.
        if (local_data->time != extrapolator.GetLastPoseTime())
        {
            if (scan_matched_publisher_->get_subscription_count() > 0)
            {
                sensor::TimedPointCloud point_cloud;
                point_cloud.reserve(local_data->range_data_in_local.returns.size());
                // cartographer::sensor::RangefinderPoint
                for (const auto &point : local_data->range_data_in_local.returns)
                {
                    point_cloud.push_back(cartographer::sensor::ToTimedRangefinderPoint(point, 0.f /* time */));
                }
                scan_matched_publisher_->publish(ToPointCloud2Message(
                    common::ToUniversal(local_data->time), map_frame,
                    sensor::TransformTimedPointCloud(point_cloud, trajectory_data.local_to_map.cast<float>())));
            }
            extrapolator.AddPose(local_data->time, local_data->local_pose);
        }

        geometry_msgs::msg::TransformStamped stamped_tf;
        // If we do not publish a new point cloud, we still allow time of the published poses to advance.
        // If we already know a newer pose, we use its time instead. Since tf knows how to interpolate,
        // providing newer information is better.
        // 这里的通过ros的now()获取的时间可能与数据集里的时间相差很大，所以尽量把use_pose_extrapolator设为false;
        const auto &time_now = std::max(FromRos(node_->now()), extrapolator.GetLastExtrapolatedTime());
        stamped_tf.header.stamp = use_pose_extra ? ToRos(time_now) : ToRos(local_data->time);
        // Suppress publishing if we already published a transform at this time.
        // Due to 2020-07 changes to geometry2, tf buffer will issue warnings for
        // repeated transforms with the same timestamp.
        // 避免发布相同时间戳的tf
        if (last_published_tf_stamps_.count(traj_id) && last_published_tf_stamps_[traj_id] == stamped_tf.header.stamp)
            return;
        last_published_tf_stamps_[traj_id] = stamped_tf.header.stamp;
        // 传入PoseExtrapolator的传感器数据都已变换到tracking系，所谓的local坐标系就是该条估计起始时刻的坐标系
        // 根据下面tf的发布逻辑，local指的就是odom系;
        const auto &local_pose = local_data->local_pose;
        const auto &extra_pose = extrapolator.ExtrapolatePose(time_now);
        const auto tracking_to_local_3d = use_pose_extra ? extra_pose : local_pose;
        const auto tracking_to_local = [&] {
            if (traj_options.publish_frame_projected_to_2d)
            {
                return transform::Embed3D(transform::Project2D(tracking_to_local_3d));
            }
            return tracking_to_local_3d;
        }();
        const auto tracking_to_map = trajectory_data.local_to_map * tracking_to_local;
        if (trajectory_data.published_to_tracking == nullptr)
            return;
        if (node_options_.publish_to_tf)
        {
            if (traj_options.provide_odom_frame)
            {
                std::vector<geometry_msgs::msg::TransformStamped> tf_set;
                stamped_tf.header.frame_id = map_frame;
                stamped_tf.child_frame_id = traj_options.odom_frame;
                stamped_tf.transform = ToGeometryMsgTransform(trajectory_data.local_to_map);
                tf_set.push_back(stamped_tf);

                stamped_tf.header.frame_id = traj_options.odom_frame;
                stamped_tf.child_frame_id = traj_options.published_frame;
                stamped_tf.transform = ToGeometryMsgTransform(tracking_to_local * (*trajectory_data.published_to_tracking));
                tf_set.push_back(stamped_tf);
                tf_broadcaster_->sendTransform(tf_set);
                // Print("local_to_map", trajectory_data.local_to_map);
                // Print("tracking_to_local", tracking_to_local);
                // Print("published_to_tracking", *(trajectory_data.published_to_tracking));
            }
            else
            {
                stamped_tf.header.frame_id = map_frame;
                stamped_tf.child_frame_id = traj_options.published_frame;
                stamped_tf.transform = ToGeometryMsgTransform(tracking_to_map * (*trajectory_data.published_to_tracking));
                tf_broadcaster_->sendTransform(stamped_tf);
                // Print("tracking_to_map", tracking_to_map);
                // Print("published_to_tracking", *(trajectory_data.published_to_tracking));
            }
        }
        if (node_options_.publish_tracked_pose)
        {
            ::geometry_msgs::msg::PoseStamped pose_msg;
            pose_msg.header.frame_id = map_frame;
            pose_msg.header.stamp = stamped_tf.header.stamp;
            pose_msg.pose = ToGeometryMsgPose(tracking_to_map);
            tracked_pose_publisher_->publish(pose_msg);
        }
    }
} // namespace cartographer_ros